
#pragma once

#include <fStream>
#include "array.h"
#include "util.h"

namespace ors
{

	//===========================================================================
	//! a 3D vector (double[3])
	struct Vector{
		double p[3];

		Vector(){}
		Vector(double x,double y,double z){ set(x,y,z); }
		double& operator()(int);
		const double& operator()(int) const;

		void set(double,double,double);
		void set(double*);
		void setZero();
		void setRandom();
		void add     (double,double,double);
		void subtract(double,double,double);
		void normalize();
		void setLength(double);
		void makeNormal  (const Vector&);
		void makeColinear(const Vector&);

		bool isZero() const;
		bool isNormalized() const;
		double isColinear(const Vector&) const;
		double length() const;
		double lengthSqr() const;
		double angle(const Vector&) const;
		double radius() const;
		double phi() const;
		double theta() const;

		void write(std::ostream&) const;
		void read (std::istream&);
	};

	//===========================================================================
	//! a matrix in 3D (double[9])
	struct Matrix{
		double p[9];

		Matrix(){};
		double& operator()(int,int);
		const double& operator()(int,int) const;

		void setZero();
		void setId();
		void setFrame   (Vector&,Vector&,Vector&);
		void setInvFrame(Vector&,Vector&,Vector&);
		void setXrot(double);
		void setSkew(const Vector&);
		void setExponential(const Vector&);
		void setOdeMatrix(double*);
		void setTensorProduct(const Vector&,const Vector&);

		void write(std::ostream&) const;
		void read (std::istream&);
	};

	//===========================================================================
	//! a quaterion (double[4])
	struct Quaternion{
		double p[4];

		Quaternion();

		void set(double q0,double x,double y,double z);
		void set(double* q);
		void setZero();
		void setRandom();
		void setDeg(double degree ,double axis0,double axis1,double axis2);
		void setDeg(double degree ,const Vector& axis);
		void setRad(double radians,double axis0,double axis1,double axis2);
		void setRad(double radians,const Vector& axis);
		void setRad(double angle);
		void setRadX(double angle);
		void setRadY(double angle);
		void setQuat(double s,double x,double y,double z);
		void setVec(Vector w);
		void setMatrix(double* m);
		void setDiff(const Vector& from,const Vector& to);
		void setInterpolate(double t,const Quaternion& a,const Quaternion b);
		void invert();
		void normalize();
		void multiply(double f);
		void alignWith(const Vector& v);

		bool isZero() const;
		bool isNormalized() const;
		double getDeg() const;
		double getRad() const;
		void getDeg(double& degree,Vector& axis) const;
		void getRad(double& angle ,Vector& axis) const;
		Vector& getVec(Vector& v) const;
		Vector& getX  (Vector& Rx) const;
		Vector& getY  (Vector& Ry) const;
		Vector& getZ  (Vector& Rz) const;
		double* getMatrix(double* m) const;
		double* getMatrixOde(double* m) const; //in Ode foramt: 3x4 memory storae
		double* getMatrixGL(double* m) const; //in OpenGL format: transposed 4x4 memory storage

		void writeNice(std::ostream& os) const;
		void write(std::ostream& os) const;
		void read(std::istream& is);
	};

	//===========================================================================
	//! a transformation in 3D (position, orientation, linear & angular velocities)
	struct Transformation{
		Vector pos;     //!< position (translation)
		Quaternion rot; //!< orientation
		Vector vel;     //!< linear velocity
		Vector angvel;  //!< angular velocity

		Transformation();

		void setZero();
		Transformation& setText(const char* txt);
		void setRandom();
		void setInverse(const Transformation& f);
		void setDifference(const Transformation& from,const Transformation& to);
		void setAffineMatrix(const double *m);

		void addRelativeTranslation   (double x,double y,double z);
		void addRelativeVelocity      (double x,double y,double z);
		void addRelativeAngVelocityDeg(double degree,double x,double y,double z);
		void addRelativeAngVelocityRad(double rad,double x,double y,double z);
		void addRelativeAngVelocityRad(double wx,double wy,double wz);
		void addRelativeRotationDeg   (double degree,double x,double y,double z);
		void addRelativeRotationRad   (double rad,double x,double y,double z);
		void addRelativeRotationQuat  (double s,double x,double y,double z);

		void appendTransformation(const Transformation& f);     // this = this * f
		void appendInvTransformation(const Transformation& f);     // this = this * f^{-1}
		void prependTransformation(const Transformation& f);         // this = f * this
		void prependInvTransformation(const Transformation& f);    // this = f^{-1} * this

		double* getAffineMatrix(double *m) const;         // 4x4 matrix with 3x3=rotation and right-column=translation
		double* getInverseAffineMatrix(double *m) const;  // 4x4 matrix with 3x3=R^{-1}   and bottom-row=R^{-1}*translation
		double* getAffineMatrixGL(double *m) const;       // in OpenGL format (transposed memory storage!!)
		double* getInverseAffineMatrixGL(double *m) const;// in OpenGL format (transposed memory storage!!)

		void write(std::ostream& os) const;
		void read(std::istream& is);
	};

}

//===========================================================================
//
// operators
//

namespace ors{
  double  operator*(const Vector&,const Vector&);
  Vector  operator^(const Vector&,const Vector&);
  Vector  operator+(const Vector&,const Vector&);
  Vector  operator-(const Vector&,const Vector&);
  Vector  operator*(double,const Vector&);
  Vector  operator*(const Vector&,double);
  Vector  operator/(const Vector&,double);
  Vector& operator*=(Vector&,double);
  Vector& operator/=(Vector&,double);
  Vector& operator+=(Vector&,const Vector&);
  Vector& operator-=(Vector&,const Vector&);
  Vector  operator-(const Vector&);

  Matrix  operator*(const Matrix& b,const Matrix& c);
  Matrix  operator+(const Matrix& b,const Matrix& c);
  Vector  operator*(const Matrix& b,const Vector& c);
  Matrix& operator*=(Matrix& a,double c);
  Matrix  operator*(double b,const Matrix& c);
  Matrix& operator+=(Matrix& a,const Matrix& b);

  Quaternion operator*(const Quaternion& b,const Quaternion& c);
  Quaternion operator/(const Quaternion& b,const Quaternion& c);
  Vector operator*(const Quaternion& b,const Vector& c);
  Vector operator/(const Quaternion& b,const Vector& c);
  Vector operator*(const Transformation& b,const Vector& c);
  Vector operator/(const Transformation& b,const Vector& c);
}
std::istream& operator>>(std::istream&,ors::Vector&);
std::istream& operator>>(std::istream&,ors::Matrix&);
std::istream& operator>>(std::istream&,ors::Quaternion&);
std::istream& operator>>(std::istream&,ors::Transformation&);
std::ostream& operator<<(std::ostream&,const ors::Vector&);
std::ostream& operator<<(std::ostream&,const ors::Matrix&);
std::ostream& operator<<(std::ostream&,const ors::Quaternion&);
std::ostream& operator<<(std::ostream&,const ors::Transformation&);

double scalarProduct(const ors::Quaternion& a,const ors::Quaternion& b);


//===========================================================================
//
// constants
//

extern const ors::Vector VEC_x;
extern const ors::Vector VEC_y;
extern const ors::Vector VEC_z;